﻿#include "TemplateMatch.h"
#include <iostream>
#include <limits>
//#include <QDebug>
#include <opencv2/highgui.hpp>
#include "ImageRotate.h"

namespace Qly {

inline void removeInvalidData(cv::Mat &result)
{
    for(int i = 0; i < result.rows; i++)
    {
         float* pLine = result.ptr<float>(i);
         for(int j = 0; j < result.cols; j++)
         {
             if(isinf(pLine[j]) || isnan(pLine[j]))
             {
                 pLine[j] = 0.0;
             }
         }
    }
}

inline void removeBigData(cv::Mat &result)
{
    for(int i = 0; i < result.rows; i++)
    {
         float* pLine = result.ptr<float>(i);
         for(int j = 0; j < result.cols; j++)
         {
             if(pLine[j] > 1e11)
             {
                 pLine[j] = 0.0;
             }
         }
    }
}

/**
 * @brief templateMatchRaw 最最原始的模板匹配方法
 * @param image 输入图像
 * @param tepl 模板图像
 * @param method 匹配方法，具体参见 cv::matchTemplate() 函数
 * @param point 返回匹配位置
 * @return 返回匹配分数，DIFF 类匹配数值越小表示匹配越好。COFF 类匹配数值越大越好。
 */
double templateMatch(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method, cv::Point &point) noexcept
{
    int result_cols = image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED) removeInvalidData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;

    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        point = maxLoc;
        return maxVal;

    default: point = maxLoc;
        return maxVal;
    }
}

bool templateMatch(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method,
                   cv::Point &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    if(method < 0 || method > 6) return false;
    cv::Mat result;// = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED) removeInvalidData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        point = minLoc;
        score = minVal;
        break;
    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        point = maxLoc;
        score = maxVal;
        break;
    default: point = maxLoc;
        score = maxVal;
        break;
    }
    return true;
}

bool templateMatch(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method,
                   cv::Rect &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    if(method < 0 || method > 6) return false;
    cv::Mat result;// = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED || method == cv::TM_CCORR_NORMED) removeInvalidData(result);
    //if(method == cv::TM_CCORR) removeBigData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        rect = cv::Rect(minLoc, tepl.size());
        score = minVal;
        break;
    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        rect = cv::Rect(maxLoc, tepl.size());
        score = maxVal;
        break;
    default:
        rect = cv::Rect(maxLoc, tepl.size());
        score = maxVal;
        break;
    }
    return true;
}

bool templateMatchCCORR(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                        cv::Point &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    point = maxLoc;
    return true;
}

bool templateMatchCCORR_NORMED(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                               cv::Point &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR_NORMED, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    point = maxLoc;
    return true;
}

bool templateMatchCCORR(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                        cv::Rect &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    rect = cv::Rect(maxLoc, tepl.size());
    return true;
}

bool templateMatchCCORR_NORMED(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                               cv::Rect &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR_NORMED, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    rect = cv::Rect(maxLoc, tepl.size());
    return true;
}


bool templateMatchCCOEFF_NORMED(const cv::Mat &image, const cv::Mat &model, const cv::Mat &mask,
                                cv::Point &point, double &score) noexcept
{
    if(image.empty()) return false;
    if(model.empty()) return false;
    if(!mask.empty() && mask.size() != model.size()) return false;
    if(image.rows < model.rows) return false;
    if(image.cols < model.cols) return false;

    double minVal = 1, maxVal = -1;
    cv::Point minLoc, maxLoc;
    cv::Mat result;
    cv::matchTemplate(image, model, result, cv::TM_CCOEFF_NORMED, mask);

    removeInvalidData(result);
    cv::minMaxLoc(result,  &minVal, &maxVal, &minLoc, &maxLoc);
    score = maxVal;
    point = maxLoc;
    return true;

}

bool templateMatchCCOEFF_NORMED(const cv::Mat &image, const cv::Mat &model, const cv::Mat &mask,
                                cv::Rect &rect, double &score) noexcept
{
    if(image.empty()) return false;
    if(model.empty()) return false;
    if(!mask.empty() && mask.size() != model.size()) return false;
    if(image.rows < model.rows) return false;
    if(image.cols < model.cols) return false;

    double minVal = 1, maxVal = -1;
    cv::Point minLoc, maxLoc;
    cv::Mat result;
    cv::matchTemplate(image, model, result, cv::TM_CCOEFF_NORMED, mask);

    removeInvalidData(result);
    cv::minMaxLoc(result,  &minVal, &maxVal, &minLoc, &maxLoc);
    score = maxVal;

    rect = cv::Rect(maxLoc, model.size());
    return true;
}

cv::Point2f subPixEstimate(const cv::Point &point, const cv::Mat &result)
{
    cv::Point2f pt2f;
    if(point.x == 0 || point.x == result.cols - 1)
    {
        pt2f.x = point.x;
    }
    else
    {
        double a, b, c; //二次函数

        c = result.at<float>(point.x + 1, point.y);
        a = - result.at<float>(point.x, point.y) + (result.at<float>(point.x - 1, point.y) + result.at<float>(point.x + 1, point.y)) / 2.0;
        b = (result.at<float>(point.x + 1, point.y) - result.at<float>(point.x - 1, point.y)) / 2.0;
        pt2f.x = point.x - b/ (a * 2.0);
    }
    if(point.y == 0 || point.y == result.rows - 1)
    {
        pt2f.y = point.y;
    }
    else
    {
        double a, b, c; //二次函数

        c = result.at<float>(point.x, point.y + 1);
        a = - result.at<float>(point.x, point.y) + (result.at<float>(point.x, point.y - 1) + result.at<float>(point.x, point.y + 1)) / 2.0;
        b = (result.at<float>(point.x, point.y + 1) - result.at<float>(point.x, point.y - 1)) / 2.0;
        pt2f.y = point.y - b/ (a * 2.0);
    }
    return pt2f;
}

bool templateMatchSubPixel(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method, cv::Point2f &point, double &score) noexcept
{
    point = cv::Point2f(0.0, 0.0);
    score = 0;
    if(image.empty() || tepl.empty() ) return false;
    if(method < 0 || method > 6) return false;
    cv::Mat result;// = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED) removeInvalidData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        point = subPixEstimate(minLoc, result);
        score = minVal;
        break;
    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        point = maxLoc;
        score = maxVal;
        break;
    default: point = subPixEstimate(maxLoc, result);
        score = maxVal;
        break;
    }
    return true;
    ///TODO： 还没有测试
}

bool templateMatchSubPixel(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method, cv::Rect2f &rect, double &score) noexcept
{
    rect = cv::Rect2f(0.0, 0.0, 0.0, 0.0);
    score = 0;
    if(image.empty() || tepl.empty() ) return false;
    if(method < 0 || method > 6) return false;
    cv::Mat result;// = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED) removeInvalidData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    cv::Point2f point;
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        point = subPixEstimate(minLoc, result);
        score = minVal;
        break;
    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        point = maxLoc;
        score = maxVal;
        break;
    default: point = subPixEstimate(maxLoc, result);
        score = maxVal;
        break;
    }
    rect.x = point.x;
    rect.y = point.y;
    rect.width = tepl.cols;
    rect.height = tepl.rows;
    return true;
    ///TODO: 还没有测试
    ///
}

bool templateMatchSubPixelCCORR_NORMED(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, cv::Point2f &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR_NORMED, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    point = subPixEstimate(maxLoc, result);
    //point = maxLoc;
    return true;
}

bool templateMatchSubPixelCCORR_NORMED(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, cv::Rect2f &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR_NORMED, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    cv::Point2f point = subPixEstimate(maxLoc, result);
    rect = cv::Rect2f(point, tepl.size());
    return true;
}

bool templateMatchSubPixelCCOEFF_NORMED(const cv::Mat &image, const cv::Mat &model, const cv::Mat &mask, cv::Point2f &point, double &score) noexcept
{
    if(image.empty()) return false;
    if(model.empty()) return false;
    if(!mask.empty() && mask.size() != model.size()) return false;
    if(image.rows < model.rows) return false;
    if(image.cols < model.cols) return false;

    double minVal = 1, maxVal = -1;
    cv::Point minLoc, maxLoc;
    cv::Mat result;
    cv::matchTemplate(image, model, result, cv::TM_CCOEFF_NORMED, mask);

    removeInvalidData(result);
    cv::minMaxLoc(result,  &minVal, &maxVal, &minLoc, &maxLoc);
    score = maxVal;
    point = subPixEstimate(maxLoc, result);
    //point = maxLoc;
    return true;
}

bool templateMatchSubPixelCCOEFF_NORMED(const cv::Mat &image, const cv::Mat &model, const cv::Mat &mask, cv::Rect2f &rect, double &score) noexcept
{
    if(image.empty()) return false;
    if(model.empty()) return false;
    if(!mask.empty() && mask.size() != model.size()) return false;
    if(image.rows < model.rows) return false;
    if(image.cols < model.cols) return false;

    double minVal = 1, maxVal = -1;
    cv::Point minLoc, maxLoc;
    cv::Mat result;
    cv::matchTemplate(image, model, result, cv::TM_CCOEFF_NORMED, mask);

    removeInvalidData(result);
    cv::minMaxLoc(result,  &minVal, &maxVal, &minLoc, &maxLoc);
    score = maxVal;
    cv::Point2f point = subPixEstimate(maxLoc, result);
    rect = cv::Rect2f(point, model.size());
    //rect = cv::Rect(maxLoc, model.size());
    return true;
}

bool templateMatchSubPixelCCORR(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, cv::Rect2f &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    cv::Point2f point = subPixEstimate(maxLoc, result);
    rect = cv::Rect2f(point, tepl.size());
    //rect = cv::Rect(maxLoc, tepl.size());
    return true;
}

bool templateMatchSubPixelCCORR(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, cv::Point2f &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    point = subPixEstimate(maxLoc, result);
    return true;
}


}








